#include "common_inc.h"
#include "user_define.h"
#include "interface_uart.hpp"
#include "ascii_processor.hpp"
#include "fibre/protocol.hpp"
#include "usart.h"
#include "main.h"
#include "CircleBuffer.h"
#include "w25qxx.h"
#include "time.h"
#include <chrono>
#include <iostream>
#include <string>
#include <sstream>
#include <iomanip>
#include <ctime>
#include "task.h"
#include "DS3231.h"
#include "bsp_i2c_ds3231.h"

using namespace std;
using namespace std::chrono;

#define UART_TX_BUFFER_SIZE 110
#define UART_RX_BUFFER_SIZE 100

extern void AnaCommand(uint8_t* buff,uint16_t len,StreamSink &output);

extern DummyRobot dummy;
extern StreamSink *DrawJointAndPosOutputPtr;
extern bool DrawJointAndPosFlag,PrintBatMsgFlag;
extern uint16_t DrawGapMs;
extern MPU6050 mpu6050;

// DMA open loop continous circular buffer
// 1ms delay periodic, chase DMA ptr around
static uint8_t dma_rx_buffer[3][UART_RX_BUFFER_SIZE];
static uint32_t dma_last_rcv_idx[3];

// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
// static thread_local uint32_t deadline_ms = 0;

osThreadId_t uartServerTaskHandle;
osThreadId_t DoInTimeTaskHandle;

class UART1Sender : public StreamSink {
public:
    UART1Sender() {
        channelType = CHANNEL_TYPE_UART1;
    }

    int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes) override {
        // Loop to ensure all bytes get sent
        WriteCircleBuff(&Uart1TxdCirclebuff,(char *)buffer,length);
            return 0;
        if(IsUart1DmaSending())
            return -1;
        while (length) {
            size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
            // wait for USB interface to become ready
            // TODO: implement ring buffer to get a more continuous stream of data
            // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
           // if (osSemaphoreAcquire(sem_uart1_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
          //      return -1;

            // transmit chunk
            memcpy(tx_buf_, buffer, chunk);
//            if (HAL_UART_Transmit_DMA(&huart1, tx_buf_, chunk) != HAL_OK)
//                return -1;
            Uart1DMASendingFlag = true;
            buffer += chunk;
            length -= chunk;
            if (processed_bytes)
                *processed_bytes += chunk;
        }
        return 0;
    }

    size_t get_free_space() override { return SIZE_MAX; }

private:
    uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart1_stream_output;


class UART4Sender : public StreamSink {
public:
    UART4Sender() {
        channelType = CHANNEL_TYPE_UART4;
    }

    int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes) override {
        // Loop to ensure all bytes get sent
        while (length) {
            size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
            // wait for USB interface to become ready
            // TODO: implement ring buffer to get a more continuous stream of data
            // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
            if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
                return -1;
            // transmit chunk
            memcpy(tx_buf_, buffer, chunk);
//            if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
//                return -1;
            buffer += chunk;
            length -= chunk;
            if (processed_bytes)
                *processed_bytes += chunk;
        }
        return 0;
    }

    size_t get_free_space() override { return SIZE_MAX; }

private:
    uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart4_stream_output;

class UART5Sender : public StreamSink {
public:
    UART5Sender() {
        channelType = CHANNEL_TYPE_UART5;
    }

    int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes) override {
        // Loop to ensure all bytes get sent
        WriteCircleBuff(&Uart5TxdCirclebuff,(char *)buffer,length);
            return 0;

        if(IsUart5DmaSending())
            return false;
        while (length) {
            size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
            // wait for USB interface to become ready
            // TODO: implement ring buffer to get a more continuous stream of data
            // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
            //if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
            //    return -1;
            // transmit chunk
            memcpy(tx_buf_, buffer, chunk);
//            if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK)
//                return -1;
            Uart5DMASendingFlag = true;
            buffer += chunk;
            length -= chunk;
            if (processed_bytes)
                *processed_bytes += chunk;
        }
        return 0;
    }

    size_t get_free_space() override { return SIZE_MAX; }

private:
    uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart5_stream_output;

StreamSink *uart1StreamOutputPtr = &uart1_stream_output;
StreamBasedPacketSink uart1_packet_output(uart1_stream_output);
BidirectionalPacketBasedChannel uart1_channel(uart1_packet_output);
StreamToPacketSegmenter uart1_stream_input(uart1_channel);

StreamSink *uart4StreamOutputPtr = &uart4_stream_output;
StreamBasedPacketSink uart4_packet_output(uart4_stream_output);
BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output);
StreamToPacketSegmenter uart4_stream_input(uart4_channel);

StreamSink *uart5StreamOutputPtr = &uart5_stream_output;
StreamBasedPacketSink uart5_packet_output(uart5_stream_output);
BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output);
StreamToPacketSegmenter uart5_stream_input(uart5_channel);

static void UartServerTask(void *ctx) {
//    (void) ctx;
//
//    for (;;) {
//        // Check for UART errors and restart recieve DMA transfer if required
//        if (huart4.ErrorCode != HAL_UART_ERROR_NONE) {
//            HAL_UART_AbortReceive(&huart4);
//            HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0]));
//        }
//        // Fetch the circular buffer "write pointer", where it would write next
//        uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
//
//        // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
//        // Process bytes in one or two chunks (two in case there was a wrap)
//        if (new_rcv_idx < dma_last_rcv_idx[0]) {
//            uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0],
//                                             UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0],
//                                             nullptr); // TODO: use process_all
//            ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
//                                        UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], uart4_stream_output);
//            dma_last_rcv_idx[0] = 0;
//        }
//        if (new_rcv_idx > dma_last_rcv_idx[0]) {
//            uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0],
//                                             new_rcv_idx - dma_last_rcv_idx[0],
//                                             nullptr); // TODO: use process_all
//            ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
//                                        new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output);
//            dma_last_rcv_idx[0] = new_rcv_idx;
//        }
//
//
//        // Check for UART errors and restart recieve DMA transfer if required
//        /* if (huart5.ErrorCode != HAL_UART_ERROR_NONE) {
//              HAL_UART_AbortReceive(&huart5);
//              HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1]));
//          }
//          // Fetch the circular buffer "write pointer", where it would write next
//            new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
//
//                 // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
//                 // Process bytes in one or two chunks (two in case there was a wrap)
//                 if (new_rcv_idx < dma_last_rcv_idx[1]) {
//                     uart5_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1],
//                                                      UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1],
//                                                      nullptr); // TODO: use process_all
//                     ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
//                                                 UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], uart5_stream_output);
//                     dma_last_rcv_idx[1] = 0;
//                 }
//                 if (new_rcv_idx > dma_last_rcv_idx[1]) {
//                     uart5_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1],
//                                                      new_rcv_idx - dma_last_rcv_idx[1],
//                                                      nullptr); // TODO: use process_all
//                     ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
//                                                 new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output);
//                     dma_last_rcv_idx[1] = new_rcv_idx;
//                 }*/
//
//
//        // Check for UART errors and restart recieve DMA transfer if required
//        /*if (huart1.ErrorCode != HAL_UART_ERROR_NONE) {
//            HAL_UART_AbortReceive(&huart1);
//            HAL_UART_Receive_DMA(&huart1, dma_rx_buffer[2], sizeof(dma_rx_buffer[2]));
//        }
//        // Fetch the circular buffer "write pointer", where it would write next
//        new_rcv_idx = UART_RX_BUFFER_SIZE - huart1.hdmarx->Instance->NDTR;
//
//        // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
//        // Process bytes in one or two chunks (two in case there was a wrap)
//        if (new_rcv_idx < dma_last_rcv_idx[2]) {
//            uart1_stream_input.process_bytes(dma_rx_buffer[2] + dma_last_rcv_idx[2],
//                                             UART_RX_BUFFER_SIZE - dma_last_rcv_idx[2],
//                                             nullptr); // TODO: use process_all
//            ASCII_protocol_parse_stream(dma_rx_buffer[2] + dma_last_rcv_idx[2],
//                                        UART_RX_BUFFER_SIZE - dma_last_rcv_idx[2], uart1_stream_output);
//            dma_last_rcv_idx[2] = 0;
//        }
//        if (new_rcv_idx > dma_last_rcv_idx[2]) {
//            uart1_stream_input.process_bytes(dma_rx_buffer[2] + dma_last_rcv_idx[2],
//                                             new_rcv_idx - dma_last_rcv_idx[2],
//                                             nullptr); // TODO: use process_all
//            ASCII_protocol_parse_stream(dma_rx_buffer[2] + dma_last_rcv_idx[2],
//                                        new_rcv_idx - dma_last_rcv_idx[2], uart1_stream_output);
//            dma_last_rcv_idx[2] = new_rcv_idx;
//        }*/
//
//
//
//        osDelay(1);
//    };
}


//打包定位数据并发给匿名上位机，调试用
void SendTarAndCurrJointToANOTech(StreamSink &output)
{

    uint8_t i,index = 0;
    uint8_t sum=0;
    uint8_t buff[256];


    union result
    {
        float fData;
        unsigned char fArray[4];
    }data;


    buff[index++] = 0xAA;
    buff[index++] = 0xAA;
    buff[index++] = 0xF1;
    buff[index++] = 0;		//数据长度


    //目标角度
    for(i=0;i<6;i++)
    {
        data.fData = (float)dummy.targetJoints.a[i];
        buff[index++] = data.fArray[3];
        buff[index++] = data.fArray[2];
        buff[index++] = data.fArray[1];
        buff[index++] = data.fArray[0];
    }

    //当前角度
    for(i=0;i<6;i++)
    {
        data.fData = (float)dummy.currentJoints.a[i];
        buff[index++] = data.fArray[3];
        buff[index++] = data.fArray[2];
        buff[index++] = data.fArray[1];
        buff[index++] = data.fArray[0];
    }


    buff[3] = index - 4;		//数据长度

    for(i=0,sum = 0;i<index;i++)
        sum+=buff[i];

    buff[index] = sum;

    output.process_bytes((const uint8_t *)buff,index+1,nullptr);

}

//打包定位数据并发给匿名上位机，调试用
void SendTarAndCurrPositionToANOTech(StreamSink &output)
{

    uint8_t i,index = 0;
    uint8_t sum=0;
    uint8_t buff[256];


    union result
    {
        float fData;
        unsigned char fArray[4];
    }data;


    buff[index++] = 0xAA;
    buff[index++] = 0xAA;
    buff[index++] = 0xF2;
    buff[index++] = 0;		//数据长度


    data.fData = (float)dummy.currentPose6D.X;
    buff[index++] = data.fArray[3];
    buff[index++] = data.fArray[2];
    buff[index++] = data.fArray[1];
    buff[index++] = data.fArray[0];

    data.fData = (float)dummy.currentPose6D.Y;
    buff[index++] = data.fArray[3];
    buff[index++] = data.fArray[2];
    buff[index++] = data.fArray[1];
    buff[index++] = data.fArray[0];

    data.fData = (float)dummy.currentPose6D.Z;
    buff[index++] = data.fArray[3];
    buff[index++] = data.fArray[2];
    buff[index++] = data.fArray[1];
    buff[index++] = data.fArray[0];

    data.fData = (float)dummy.currentPose6D.A;
    buff[index++] = data.fArray[3];
    buff[index++] = data.fArray[2];
    buff[index++] = data.fArray[1];
    buff[index++] = data.fArray[0];

    data.fData = (float)dummy.currentPose6D.B;
    buff[index++] = data.fArray[3];
    buff[index++] = data.fArray[2];
    buff[index++] = data.fArray[1];
    buff[index++] = data.fArray[0];

    data.fData = (float)dummy.currentPose6D.C;
    buff[index++] = data.fArray[3];
    buff[index++] = data.fArray[2];
    buff[index++] = data.fArray[1];
    buff[index++] = data.fArray[0];


    buff[3] = index - 4;		//数据长度

    for(i=0,sum = 0;i<index;i++)
        sum+=buff[i];

    buff[index] = sum;

    output.process_bytes((const uint8_t *)buff,index+1,nullptr);

}

bool IsUart1DmaSending(){
    if(Uart1DMASendingFlag == false)
        return false;
    else
        return true;
}

bool IsUart5DmaSending(){
    if(Uart5DMASendingFlag == false)
        return false;
    else
        return true;
}

bool TimesToTimestampMs(char* timeStr,long long* TimestampMs){
    tm tmTime = {};
    istringstream ss((string)timeStr);
    ss >> std::get_time(&tmTime, "%Y-%m-%d %H:%M:%S");
    int MsVal = atoi(strstr(timeStr,".")+1);

    auto timePoint = std::chrono::system_clock::from_time_t(std::mktime(&tmTime));
    *TimestampMs = std::chrono::duration_cast<std::chrono::milliseconds>(timePoint.time_since_epoch()).count();
    *TimestampMs += MsVal;
    return true;
}

bool TimestampToTimesMs(long long TimestampMs,char* timeStr){
    long long Timestamp = TimestampMs/1000;
    tm* timeinfo = localtime(&Timestamp);
    strftime(timeStr, 80, "%Y-%m-%d %H:%M:%S", timeinfo);
    sprintf(&timeStr[strlen(timeStr)],".%03lld",TimestampMs%1000);
    return true;
}

bool WriteLog(ROBOT_LOG_TYPE type,char* msg,uint16_t size){
    long long time;
    char TimeS[30];
    LogMsgWrite.RobotLogType = type;
    LogMsgWrite.LogMsgSize = size;
    LogMsgWrite.LogMsg = msg;
    //LogMsgWrite.LogNum = LastLogNum++;
    LogMsgWrite.PowerOnTimes = HAL_GetTick();
    sprintf(TimeS,"%02d-%02d-%02d %02d:%02d:%02d.%03d",calendar.year,calendar.month,calendar.day,calendar.hour,calendar.min,calendar.sec,HAL_GetTick()%1000);
    TimesToTimestampMs(TimeS,&time);
    LogMsgWrite.NetTimes = time;

    LogMsgWrite.LogSize = (uint32_t)&(LogMsgWrite.LogEnd) - (uint32_t)&(LogMsgWrite.LogHead) + 4 + LogMsgWrite.LogMsgSize - 4;
    LogMsgWrite.CrcSum = 0;

    return true;

}

extern void SendRobotMsgToComputer(StreamSink &output);
extern COLOR_RGBC rgb;
extern BAT_MSG BatMsg[17][25];
//extern DOF6Kinematic::Joint6D_t LastJointToTarDiffMin,JointToTarDiffFabs;

void ConsoleTask(void *ctx)
{
	(void) ctx;
    static bool GapConFlag = true,ConveyorBeltConFlag = false;
    uint32_t tick;
    static uint32_t ConsoleTimesMs;
    static DOF6Kinematic::Joint6D_t LastJoints;

	for(;;){
        tick = osKernelGetTickCount();
        ConsoleTimesMs+=10;
        RobotRunTimesMs+=10;


        if(RobotSta == ROBOT_CONTROL_WITH_COMMAND || RobotSta == ROBOT_WORK_WITH_PROCEDRUE){
            for (int i = 0; i < dummy.RobotAxisNum; ++i) {
                //JointToTarDiffFabs.a[i] = fabs(dummy.targetJoints.a[i] - dummy.currentJoints.a[i]);
            }

            for (int i = 0; i < dummy.RobotAxisNum; ++i) {
                //if(JointToTarDiffFabs.a[i] > 0.002){
                    if(RunToTarTimesMs == 0){
                        //LastJointToTarDiffMin = JointToTarDiffFabs;
                        BlockedTimesMs = 0;
                        LastJoints = dummy.currentJoints;
                    }
                    RunToTarTimesMs+=10;
                    break;
                //}
            }
#ifndef IMU6_T
            if(dummy.HarVer==BATTLE_CHARGE_THERE_AXIS){
                if(RunToTarTimesMs % 1000 == 0){
                    if(RunToTarTimesMs > 1000){
                        for (int i = 0; i < dummy.RobotAxisNum; ++i) {
                            if(IsTarAdd[i] == true && dummy.targetJoints.a[i] - dummy.currentJoints.a[i] > (dummy.HarVer!=BATTLE_CHARGE_THERE_AXIS?1:0.02) && dummy.currentJoints.a[i] - LastJoints.a[i] < 0.00001 ||
                               IsTarAdd[i] == false && dummy.currentJoints.a[i] - dummy.targetJoints.a[i] > (dummy.HarVer!=BATTLE_CHARGE_THERE_AXIS?1:0.02) && LastJoints.a[i] - dummy.currentJoints.a[i] < 0.00001){
                                BlockedTimesMs+=1000;
                                if(BlockedTimesMs >= 2000){
                                    //Respond(*usbStreamOutputPtr,"\nErr,i:%d d:%d t:%.4f L:%.4f C:%.4f tD:%.3f lD:%.3f\n",i,IsTarAdd[i],dummy.targetJoints.a[i],LastJoints.a[i],dummy.currentJoints.a[i],dummy.targetJoints.a[i] - dummy.currentJoints.a[i],LastJoints.a[i]-dummy.currentJoints.a[i]);
                                    sprintf((char*)SprintfBuff,"Disable,MotorId:%d diffD:%.4f",i+1,dummy.targetJoints.a[i] - dummy.currentJoints.a[i]);
                                    WriteLogToBuff(ERR_LOG,SprintfBuff);
                                    RobotDisable();
                                }else
                                    printf("\nRunToTarTimesMs:%d BlockedTimesMs:%d\n",RunToTarTimesMs,BlockedTimesMs);
                            }

                        }
                    }
                    LastJoints = dummy.currentJoints;
                }
            }
#else

#endif

//            if(RunToTarTimesMs >= 200 && RunToTarTimesMs % 100 == 0){
//                for (int i = 0; i < 6; ++i) {
//                    if(fabs(JointsDiff.a[i]) < 0.01 && fabs(JointsDiff.a[i]) > 0.1){
//                        RobotSta = ROBOT_DISABLED;
//                        printf("Err stop\n");
//                        break;
//                    }
//                    else{
//                        printf("j%d: %.3f %.5f\n",i,dummy.currentJoints.a[i],LastJoints.a[i]-dummy.currentJoints.a[i]);
//                    }
//                }
//
//                for (int i = 0; i < 6; ++i) {
//                    if
//                }
//            }
//            if(RunToTarTimesMs > RunToTarAllTimesMs*1.5){
//                RobotSta = ROBOT_DISABLED;
//              printf("Err stop\n");
//            }
        }


        if(DrawJointAndPosFlag == true && ConsoleTimesMs % DrawGapMs == 0){
            SendTarAndCurrJointToANOTech(*DrawJointAndPosOutputPtr);
            SendTarAndCurrPositionToANOTech(*DrawJointAndPosOutputPtr);
        }
        //Respond(*usbStreamOutputPtr,"dy: %.3f %.3f %.3f %.3f %.3f %.3f\n",dummy.dynamicJointSpeeds.a[0],dummy.dynamicJointSpeeds.a[1],dummy.dynamicJointSpeeds.a[2],dummy.dynamicJointSpeeds.a[3],dummy.dynamicJointSpeeds.a[4],dummy.dynamicJointSpeeds.a[5]);
        //Respond(*usbStreamOutputPtr,"Fx:%.1f Fy:%.1f Fz:%.1f M: %.2f, %.2f, %.2f, %.2f\n",mpu6050.data.axAngle, mpu6050.data.ayAngle,mpu6050.data.azAngle,MpuAngle[3],MpuAngle[4],MpuAngle[5],MpuAngle[6]);
#ifndef IMU6_T
        if(PrintBatMsgFlag && dummy.HarVer == BATTLE_CHARGE_THERE_AXIS && ConsoleTimesMs % 1000 == 0){
            Respond(*usbStreamOutputPtr,"\n");
            for (int i = 1; i <= 16; ++i) {
                Respond(*usbStreamOutputPtr,"floor:%d",i);
                for (int j = 1; j <= 24; ++j) {
                    if(j%12==1)
                        Respond(*usbStreamOutputPtr,"\n");
                    if(BatMsg[i][j].BatChargeSta == BAT_CHARGING ||  BatMsg[i][j].BatChargeSta == BAT_FULL)
                        Respond(*usbStreamOutputPtr,"%02d:%3d%%(%d %5d)\t",j,BatMsg[i][j].batteryPercent,BatMsg[i][j].BatChargeSta,RobotRunTimesMs-BatMsg[i][j].StaChargeTimes);
                    else
                        Respond(*usbStreamOutputPtr,"%02d:   %%(%d)\t",j,BatMsg[i][j].BatChargeSta);

                }
                Respond(*usbStreamOutputPtr,"\n");
            }
        }
#else

#endif

//        if (ConsoleTimesMs % 500 == 0){
//            Respond(*usbStreamOutputPtr,"HomeKeySta:%d\n",IfHomeKeyOn());
//        }
//
//        if(ConsoleTimesMs % 50 == 0){
//            dummy.motorJ[dummy.HarVer!= BATTLE_CHARGE_THERE_AXIS ?6:3]->UpdateTofData();
//        }
////
 //       if(ConsoleTimesMs % 1000 == 0){
//            Respond(*usbStreamOutputPtr,"TofDis:%d mm\n",TofDis);
            //Respond(*uart1StreamOutputPtr, "TofDis:%d mm\n",TofDis);
            //printf("ID:%04x\n",W25QXX_ReadID());
//            Respond(*uart5StreamOutputPtr, "TofDis:%d mm\n",TofDis);

           // GapCon(GapConFlag);
           // GapConFlag = !GapConFlag;
 //       }


#ifndef IMU6_T
        if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS && GettedChargePanelMsg == false && ConsoleTimesMs % 100 == 0){
            AskBatMsg(0,0);
        }

#else

#endif
//        if(dummy.HarVer == BATTLE_CHARGE_THERE_AXIS && ConsoleTimesMs % 5000 == 0){
//            AskBatMsg(1,0);
//        }

        if(ConsoleTimesMs % 1000 == 0){
            //ConveyorBeltCon(ConveyorBeltConFlag);
            //ConveyorBeltConFlag = !ConveyorBeltConFlag;
//            BeepCon(ConveyorBeltConFlag);
//            ConveyorBeltConFlag = !ConveyorBeltConFlag;
            //printf("BatStopChargePer:%d\n",BatStopChargePer);

//            char timeStr[30] = "2023-04-19 10:02:51.849";
//            long long TimestampMs;
//            TimesToTimestampMs(timeStr,&TimestampMs);
//            cout << "\nTime: " << timeStr << std::endl;
//            cout << "Timestamp: " << TimestampMs << std::endl;
//
//
//            char times[30];
//            TimestampToTimesMs(TimestampMs,times);
//            cout << "times:" << times << endl;

           // WriteLog(TYPE_C_COMMAND,timeStr,strlen(timeStr));
           // printf("LogMsgWrite.LogSize:%d\n",LogMsgWrite.LogSize);
//            long long timestamp = 1610027472000; // 时间戳，单位为毫秒
//
//            // 将时间戳转换为time_point类型
//            auto time_point = time_point<system_clock, milliseconds>(milliseconds(timestamp));
//
//            // 将time_point转换为time_t类型
//            time_t time_t_point = system_clock::to_time_t(time_point);
//
//            // 将time_t类型转换为tm结构体类型
//            tm* tm_point = localtime(&time_t_point);
//
//            // 输出转换后的时间
//            char time_str[20];
//            strftime(time_str, 20, "%Y-%m-%d %H:%M:%S", tm_point);
//            cout << time_str << "." << timestamp % 1000 << endl;

//            time_t timestamp;
//            struct tm *tm_info;
//            char buffer[26];
//            char time_str[20];
//            // 将时间字符串转换为时间戳
//            printf("请输入要转换的时间（格式为\"YYYY-MM-DD HH:MM:SS\"）：");
//            scanf("%s", time_str);
//            // 将时间字符串转换为UTC时间
//            //strptime(time_str, "%Y-%m-%d %H:%M:%S", tm_info);
//
//            // 将UTC时间转换为时间戳
//            timestamp = mktime(tm_info);
//
//            printf("转换后的时间戳为: %ld\n", timestamp);

//            timestamp = 1681869771;
//            // 将时间戳转换为UTC时间
//            tm_info = gmtime(&timestamp);
//            // 将UTC时间格式化为字符串
//            strftime(buffer, 26, "%Y-%m-%d %H:%M:%S", tm_info);
//            printf("times:%s\n", buffer);

//            WriteCircleBuff(&WriteLogCirclebuff,(char*)"0123456789",10);
//            printf("WriteLogCirclebuff.UnReadRowSum:%d\n", WriteLogCirclebuff.UnReadRowSum);
//            WriteCircleBuff(&WriteLogCirclebuff,(char*)"0123456789abcdefg",17);
//            printf("WriteLogCirclebuff.UnReadRowSum:%d\n", WriteLogCirclebuff.UnReadRowSum);

//            printf("\r\ntask_name   task_state  priority   stack  tasK_num\n");
//            //保存信息的数组
//            char InfoBuffer[1000];
//            vTaskList(InfoBuffer);							//获取所有任务的信息
//            printf("%s\r\n",InfoBuffer);					//通过串口打印所有任务的信息
//            Respond(*usbStreamOutputPtr, "[sys] Heap remain: %d Bytes\n", xPortGetMinimumEverFreeHeapSize());

            //Respond(*uart5StreamOutputPtr,"Unread:%d\n",Uart5RxdCirclebuff.UnReadRowSum);
            //Respond(*usbStreamOutputPtr,"time:%d\n",HAL_GetTick());

//            DS3231_GetTime(&rtc);
//            Respond(*usbStreamOutputPtr,"Sec:%d\n",rtc.Sec);



        }

        //Respond(*uart1StreamOutputPtr, "test\n");
        //ResponResultToComputer(buff,20,11,1);
        //Respond(*usbStreamOutputPtr,"can rgbc %d %d %d %d\n",rgb.r,rgb.g,rgb.b,rgb.c);

        osDelayUntil(tick + 10);
	}
}

void DoInTimeTask(void *ctx)
{
//    (void) ctx;
//    uint32_t tick;
//
//    uint8_t *pRead = NULL;
//    uint16_t len;
//    uint16_t idx;
//
//    for(;;){
//        tick = osKernelGetTickCount();
//
//        pRead = (uint8_t*)ReadCircleBuff_ReturnAddr(&Uart1RxdCirclebuff);
//        if(pRead != NULL){
//            len = GetThisRowDataLen(pRead);
//            pRead += 2;
//            idx = 0;
//            //Respond(*uart1StreamOutputPtr,"Unread:%d readLen:%d\n",UartRxdCirclebuff.UnReadRowSum,len);
////            Respond(*usbStreamOutputPtr,"RecLen:%d\n",len);
////            for (int i = 0; i < len; ++i) {
////                Respond(*usbStreamOutputPtr,"%02x ",pRead[i]);
////            }
//            while(IsCalSumRight((uint8_t*)&pRead[idx],pRead[idx+2]) && idx < len){
//                //AnaCommand((uint8_t*)&pRead[idx],pRead[idx+2],*uart1StreamOutputPtr);
//                idx += pRead[idx+2];
//            }
//        }
//
//        pRead = (uint8_t*)ReadCircleBuff_ReturnAddr(&Uart5RxdCirclebuff);
//        if(pRead != NULL){
//            len = GetThisRowDataLen(pRead);
//            pRead += 2;
//            idx = 0;
//            //Respond(*uart5StreamOutputPtr,"Unread:%d readLen:%d\n",Uart5RxdCirclebuff.UnReadRowSum,len);
//            while(IsCalSumRight((uint8_t*)&pRead[idx],pRead[idx+2]) && idx < len){
//                AnaCommand((uint8_t*)&pRead[idx],pRead[idx+2],*uart5StreamOutputPtr);
//                idx += pRead[idx+2];
//            }
//        }
//
//        if(huart1.gState != HAL_UART_STATE_BUSY_TX){
//            pRead = (uint8_t*)ReadCircleBuff_ReturnAddr(&Uart1TxdCirclebuff);
//            if(pRead != NULL){
//                len = GetThisRowDataLen(pRead);
//                pRead += 2;
//                HAL_UART_Transmit_DMA(&huart1,pRead,len);
//            }
//        }
//
//        if(huart5.gState != HAL_UART_STATE_BUSY_TX){
//            pRead = (uint8_t*)ReadCircleBuff_ReturnAddr(&Uart5TxdCirclebuff);
//            if(pRead != NULL){
//                len = GetThisRowDataLen(pRead);
//                pRead += 2;
//                HAL_UART_Transmit_DMA(&huart5,pRead,len);
//            }
//        }
//
//
//        osDelayUntil(tick + 1);
//    }
}


const osThreadAttr_t uartServerTask_attributes = {
        .name = "UartServerTask",
        .stack_size = 2000,
        .priority = (osPriority_t) osPriorityNormal,
};

const osThreadAttr_t ConsoleServerTask_attributes = {
        .name = "ConsoleServerTask",
        .stack_size = 4000,
        .priority = (osPriority_t) osPriorityNormal,
};


const osThreadAttr_t DoIntimeTask_attributes = {
        .name = "DoIntimeTask",
        .stack_size = 2000,
        .priority = (osPriority_t) osPriorityNormal,
};

extern void WriteLogTask(void *ctx);
void StartUartServer() {
    // DMA is set up to receive in a circular buffer forever.
    // We don't use interrupts to fetch the data, instead we periodically read
    // data out of the circular buffer into a parse buffer, controlled by a state machine
//    Respond(uart1_stream_output, "start rec cmd\n");

//    HAL_UART_AbortReceive(&huart4);
//    HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0]));
//    dma_last_rcv_idx[0] = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;

    //HAL_UART_AbortReceive(&huart5);
    //HAL_UART_DMAStop(&huart5);//停止DMA，为了重新设置DMA发送多少数据
    //HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1]));
    //0dma_last_rcv_idx[1] = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;

    //__HAL_UART_ENABLE_IT(&huart5, UART_IT_IDLE);
    //HAL_UARTEx_ReceiveToIdle_DMA(&huart5,Uart5.RX_pData,UART_RX_BUFF_SIZE);
    //HAL_UART_Receive_DMA(&huart5,Uart5.RX_pData,UART5_RX_BUFF_SIZE);

    //HAL_UART_AbortReceive(&huart1);
    //HAL_UART_Receive_DMA(&huart1, dma_rx_buffer[2], sizeof(dma_rx_buffer[2]));
    //dma_last_rcv_idx[2] = UART_RX_BUFFER_SIZE - huart1.hdmarx->Instance->NDTR;
    //HAL_UARTEx_ReceiveToIdle_DMA(&huart1,Uart1.RX_pData,UART_RX_BUFF_SIZE);

    // Start UART communication thread
    //uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes);


	// Start UART communication thread
//    uartServerTaskHandle = osThreadNew(ConsoleTask, nullptr, &ConsoleServerTask_attributes);
//    DoInTimeTaskHandle = osThreadNew(DoInTimeTask, nullptr, &DoIntimeTask_attributes);
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
//    if (huart->Instance == USART1){
//        Uart1DMASendingFlag = false;
//        osSemaphoreRelease(sem_uart1_dma);
//    }
//    else if (huart->Instance == UART4){
//        osSemaphoreRelease(sem_uart4_dma);
//    }
//    else if (huart->Instance == UART5){
//        Uart5DMASendingFlag = false;
//        #ifdef SITO_R
//            HAL_GPIO_WritePin(RS485_DIR_GPIO_Port, RS485_Dir_Pin, GPIO_PIN_RESET);
//        #endif
//        osSemaphoreRelease(sem_uart5_dma);
//    }

}
